#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>


typedef pcl::PointXYZ PointT;
ros::Publisher filterPointCloud;
pcl::Filter<PointT>::Ptr downsample_filter;
pcl::Filter<PointT>::Ptr outlier_removal_filter;

pcl::PointCloud<PointT>::Ptr  cur_cloud_Ptr(new pcl::PointCloud<PointT>);


//下采样滤波器
pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud)
{
    if(!downsample_filter) {
      return cloud;
    }

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*filtered);
    filtered->header = cloud->header;

    return filtered;
}

//野值点滤波器
double radius = 0.4;
int min_neighbors = 2;

pcl::PointCloud<PointT>::ConstPtr outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud)
{
    if(!outlier_removal_filter) {
      return cloud;
    }

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    outlier_removal_filter->setInputCloud(cloud);
    outlier_removal_filter->filter(*filtered);
    filtered->header = cloud->header;

    return filtered;
}


//距离滤波器
//距离滤波器参数
double distance_near_thresh = 1.0;
double distance_far_thresh = 15;

pcl::PointCloud<PointT>::ConstPtr distance_filter(const pcl::PointCloud<PointT>::ConstPtr& cloud)
{
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    filtered->reserve(cloud->size());

    std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points),
      [&](const PointT& p) {
        double d = p.getVector3fMap().norm();
        return d > distance_near_thresh && d < distance_far_thresh;
      }
    );

    filtered->width = filtered->size();
    filtered->height = 1;
    filtered->is_dense = false;

    filtered->header = cloud->header;

    return filtered;
}
//高度滤波器
double height = 4.0;
pcl::PointCloud<PointT>::ConstPtr height_filter(const pcl::PointCloud<PointT>::ConstPtr& cloud)
{
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    filtered->reserve(cloud->size());

    std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points),
      [&](const PointT& p) {
        double d = p.z;
        return d > -height && d < height;
      }
    );

    filtered->width = filtered->size();
    filtered->height = 1;
    filtered->is_dense = false;

    filtered->header = cloud->header;

    return filtered;
}

double downsample_resolution = 0.2;

pcl::PointCloud<PointT>::ConstPtr filter_point(const pcl::PointCloud<PointT>::Ptr& source_point)
{
    
    pcl::PointCloud<PointT>::ConstPtr filtered = distance_filter(source_point);
    ROS_INFO("distance");

    filtered = height_filter(filtered);
    ROS_INFO("Height");

    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    downsample_filter = voxelgrid;
    filtered = downsample(filtered);
    ROS_INFO("downsample");
    

    pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
    rad->setRadiusSearch(radius);
    rad->setMinNeighborsInRadius(min_neighbors);
    outlier_removal_filter = rad;
    filtered = outlier_removal(filtered);
    ROS_INFO("outlier_removal");

    return filtered;
}

void velodyne_points_cb(const sensor_msgs::PointCloud2::ConstPtr& msg)
{


	  ROS_INFO("velodyne points is receiving ");
    pcl::fromROSMsg(*msg, *cur_cloud_Ptr);

    pcl::PointCloud<PointT>::ConstPtr fliter_cloud_Ptr = filter_point(cur_cloud_Ptr);

    sensor_msgs::PointCloud2 output_;
    pcl::toROSMsg(*fliter_cloud_Ptr,output_);
    
    output_.header =msg->header;
    filterPointCloud.publish(output_);
}

int main (int argc, char** argv)
{
    ros::init (argc, argv, "pc_fliter");

    ros::NodeHandle nh; 
    ros::NodeHandle nh_p("~");


    ros::Subscriber velodynePointCloudSub = nh.subscribe ("/velodyne_points", 1, velodyne_points_cb);

    filterPointCloud = nh.advertise<sensor_msgs::PointCloud2>("filter_points", 1);



    ros::Rate r(100.0);
    while(nh.ok()){

        ros::spinOnce();
        r.sleep();
    }
    

    return 0;
}